dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * shot_local_estimator.h
   3  *
   4  *  Created on: Mar 24, 2012
   5  *      Author: aitor
   6  */
   7
   8 #pragma once
   9
  10 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
  11 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
  12 #include <pcl/features/fpfh.h>
  13
  14 namespace pcl
  15 {
  16   namespace rec_3d_framework
  17   {
  18     template<typename PointInT, typename FeatureT>
  19       class FPFHLocalEstimation : public LocalEstimator<PointInT, FeatureT>
  20       {
  21
  22         using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  23         using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  24         using KeypointCloud = pcl::PointCloud<pcl::PointXYZ>;
  25
  26         using LocalEstimator<PointInT, FeatureT>::support_radius_;
  27         using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  28         using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  29
  30       public:
  31         bool
  32         estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) override
  33         {
  34
  35           if (!normal_estimator_)
  36           {
  37             PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
  38             return false;
  39           }
  40
  41           if (keypoint_extractor_.empty ())
  42           {
  43             PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
  44             return false;
  45           }
  46
  47           //compute normals
  48           pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  49           normal_estimator_->estimate (in, processed, normals);
  50
  51           this->computeKeypoints(processed, keypoints, normals);
  52           std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
  53
  54           if (keypoints->points.empty ())
  55           {
  56             PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n");
  57             return false;
  58           }
  59
  60           assert (processed->points.size () == normals->points.size ());
  61
  62           //compute signatures
  63           using FPFHEstimator = pcl::FPFHEstimation<PointInT, pcl::Normal, pcl::FPFHSignature33>;
  64           typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
  65
  66           pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);
  67           FPFHEstimator fpfh_estimate;
  68           fpfh_estimate.setSearchMethod (tree);
  69           fpfh_estimate.setInputCloud (keypoints);
  70           fpfh_estimate.setSearchSurface(processed);
  71           fpfh_estimate.setInputNormals (normals);
  72           fpfh_estimate.setRadiusSearch (support_radius_);
  73           fpfh_estimate.compute (*fpfhs);
  74
  75           signatures->resize (fpfhs->points.size ());
  76           signatures->width = static_cast<int> (fpfhs->points.size ());
  77           signatures->height = 1;
  78
  79           int size_feat = 33;
  80           for (std::size_t k = 0; k < fpfhs->points.size (); k++)
  81             for (int i = 0; i < size_feat; i++)
  82               signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i];
  83
  84           return true;
  85
  86         }
  87
  88       };
  89   }
  90 }